// ServoCtrl.c 
// Copyright (C) 2011
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 2
// of the License, or (at your option) any later version.

// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.

// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.


///
/// \file ServoCtrl.c
/// \date 22.11.2011
/// \author cord
///
/// \date 20120321
///	\author hyla
/// \version 1.0
/// \brief adapted for LPC11U14
///

#ifdef __USE_CMSIS
#include "LPC11Uxx.h"
#endif

#define SERVOCTRL_C_

//#include "lpc_types.h"
#include "application.h"
#include "gpio.h"
#include "timer32.h"
#include "ServoCtrl.h"

volatile struct ServoCtrl_t ServoArray;

///////////////////////////////////////////////////////////////////////////////
/// \brief		TIMER32_1 interrupt handler sub-routine
/// \return 	None
///////////////////////////////////////////////////////////////////////////////

void TIMER32_1_IRQHandler(void)
{
	if ( LPC_CT32B1->IR & (0x01<<0) ) 								// MR0 Interrupt
	{
		LPC_CT32B1->IR |= 1;										// Clear Pending Interrupt

		GPIOSetBitValue(ServoArray.channel[ServoArray.idx].port, ServoArray.channel[ServoArray.idx].bit, 0);
		if(ServoArray.idx < ServoArray.maxIndex)
		{
			ServoArray.idx++;
			LPC_CT32B1->MR0 = LPC_CT32B1->TC + ServoArray.channel[ServoArray.idx].pulseLength;
			if(ServoArray.channel[ServoArray.idx].used)
			{
				GPIOSetBitValue(ServoArray.channel[ServoArray.idx].port, ServoArray.channel[ServoArray.idx].bit, 1);
			}
		}
	}
	else if ( LPC_CT32B1->IR & 2)								// MR1 Interrupt
	{
		LPC_CT32B1->IR |= 2;
		LPC_CT32B1->MR1 = LPC_CT32B1->TC + 20000;
		LPC_CT32B1->MR0 = LPC_CT32B1->TC + ServoArray.channel[0].pulseLength;
		ServoArray.idx = 0;
		GPIOSetBitValue(ServoArray.channel[0].port, ServoArray.channel[0].bit, 1);
	}
}

///////////////////////////////////////////////////////////////////////////////
/// \brief	add servo channel description
///
/// \param[in]	index	servo index (1..MAX_SERVOS)
/// \param[in]  servo   servo description
/// \return 	0, 1 for correct servo installation
///////////////////////////////////////////////////////////////////////////////
uint8_t setServo(uint8_t index, struct Servo_t servo)
{
	if(index > 0 && index<=MAX_SERVOS)
	{
		GPIOSetDir(servo.port, servo.bit, 1);			// set direction for servo-port/pin to OUT
		GPIOSetBitValue(servo.port, servo.bit, 0);		// clear servo-port/pin
		ServoArray.channel[index-1] = servo;
		ServoArray.maxIndex++;							// after finishing, maxIndex will be set to real value
		return 1;
	}
	else
	{
		return 0;
	}
}


///////////////////////////////////////////////////////////////////////////////
/// \brief	set up TIMER32_1 parameters and IRQ
///
/// \return 	None
///////////////////////////////////////////////////////////////////////////////

void initServoCtrl(void)
{
	ServoArray.maxIndex = 0;				// will be set to correct value automatically
	ServoArray.idx = 0;						// current Servo ID

	LPC_SYSCON->SYSAHBCLKCTRL |= (1<<10);	// Timer32_1 Clock enabled
	LPC_CT32B1->PC = 0;						// mystery value - what? Why? When?
	LPC_CT32B1->CTCR = 0;					// set Timer32_1 to Timer Mode
	LPC_CT32B1->PR = 47;					// empirically - results in correct timing

	LPC_CT32B1->MR0 = 5000;
	LPC_CT32B1->MR1 = 20000;
	LPC_CT32B1->MCR |= 0b011001;			// set MR0 & MR1 'Match'-parameter

	LPC_CT32B1->TCR = 1; 					// reset TMR32B1

    NVIC_SetPriority(TIMER_32_1_IRQn, ((0x01 << 3) | 0x02)); 	// preemption = 1, sub-priority = 2
    NVIC_EnableIRQ(TIMER_32_1_IRQn); 							// Enable interrupt for Timer32_1
//    LPC_TMR32B1->TCR = 1;    									// Enable Timer32_1 - REMOVED BECAUSE TOO EARLY
}
